#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <pthread.h>

#include <GL/gl.h>
#include <GL/glext.h>
#include <GL/glu.h>
#include <GL/glut.h>


#include "GMSim.hh"
#include "SimUtils.hh"
#include "GMSimPhys.hh"

GMSimPhys *phys;
char a;
float dx, dy;
pthread_t p;

void glutStuff(int ,const char** );
void display();
void timercall(int );
void initGL();
void drawCircle(int ,int ,float );
void drawRobot(int );
void drawField();
void mouseMotion(int, int);
void processMouse(int, int, int, int);
void * thread(void * ptr);

GMSim::GMSim() {
	a = 0;
	dx = 0.0;
	dy = 0.0;

	phys = new GMSimPhys();

	pthread_create(&p, NULL, thread, NULL);
}

void * thread(void * ptr) 
{
	int argc = 1;
	char *argv[] = { "gearmedula" };
	glutStuff(argc, (const char **)argv);
}

void glutStuff(int argc, const char **argv)
{
        glutInit(&argc, (char**)argv);

        glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA);

        glutInitWindowSize(840, 640);
        glutCreateWindow("sim");

        initGL();

        glutDisplayFunc(display);
        glutTimerFunc(SLEEP_TICKS, timercall, 0);
	
	glutMotionFunc(mouseMotion); 
	glutMouseFunc(processMouse);

	glutMainLoop();
}



/**
 * Desenha tudo
 **/
void display()
{
	glClear(GL_COLOR_BUFFER_BIT);

        glLoadIdentity();

	glTranslatef(-500.0f, -500.0f, 0.0f);

	//desenha campo
	drawField();

	//desenha o robô
	int i;
	for (i = 0; i < NUM_ROB; ++i)
		drawRobot(i);
	
	//desenha a bola
	glColor3f(0.9f, 0.7f, 0.2f);
	float p[2];
	phys->get_ballpos(p);
	drawCircle((int) p[0], (int) p[1], BALL_RADIUS);
	glutSwapBuffers();

	phys->update();
}

void timercall(int value)
{
        glutTimerFunc(SLEEP_TICKS, timercall, 0);
                
        glutPostRedisplay();
}

/**
 * Inicia o OpenGL
 **/
void initGL()
{
        glClearColor(0.271, 0.765, 0.102, 0.0);

        glPointSize(3.0);

        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        glOrtho(-1000.0f, 1600.0f, -1000.0, 1000.0, -1.0, 1.0);
        glScalef(2.0, 2.0, 1.0);
        glMatrixMode(GL_MODELVIEW);
	
	//BuildFont();
}

/**
 * Desenha uma circunferência.
 * x, y é o cento da circunferência.
 * r é o raio.
 **/
void drawCircle(int x, int y, float r)
{
	int i;
	float alfa;

	glBegin(GL_POLYGON);

	for (i=0; i<360; i++) {
		alfa = i*DEG2RAD;
		glVertex2f(x + cos(alfa)*r, y + sin(alfa)*r);
	}

	glEnd();
}

/**
 * Desenha o robô, que é quase uma circunferência
 * num é o identificador do robô 
 */
void drawRobot(int num)
{
	float p[2], a;
	phys->get_robotpos(p, num);
	phys->get_robotang(&a, num);

	float i;
	float alfa;

	glColor3f(0.0f, 0.0f, 0.0f);
	glBegin(GL_POLYGON);
	for (i = 35 + a*RAD2DEG; i < 326 + a*RAD2DEG; i++) {
		alfa = i*DEG2RAD;
		glVertex2f(p[0] + cos(alfa)*ROBOT_RADIUS, p[1] + sin(alfa)*ROBOT_RADIUS);
	}
	glEnd();
	
	if (num < 5) 
		glColor3f(0.2784f, 0.6431f, 0.7765f);
	else 
		glColor3f(0.9f, 0.1f, 0.1f);
	drawCircle(p[0] - (BALL_RADIUS*3.0/2.0)*cos(a),
			       p[1] - (BALL_RADIUS*3.0/2.0)*sin(a),
			       BALL_RADIUS);
	
	if (num < 5)
		glColor3f(0.2*num, 1.0 - 0.2*num, 0.4);
	else 
		glColor3f(0.2*(num-5), 1.0 - 0.2*(num-5), 0.4);
	drawCircle(p[0] + (BALL_RADIUS*3.0/2.0)*cos(a),
			       p[1] + (BALL_RADIUS*3.0/2.0)*sin(a),
			       BALL_RADIUS);
}

/**
 * Desenha o campo
 */
void drawField()
{
	glColor3f(1.0f, 1.0f, 1.0f);

	float world[28];
	phys->get_world(world);
	
	glBegin(GL_LINE_LOOP);
		glVertex2f(world[10], world[11]);
		glVertex2f(world[26], world[27]);
		glVertex2f(world[2], world[27]);
		glVertex2f(world[2], world[25]);
		glVertex2f(world[24], world[25]);
		glVertex2f(world[12], world[13]);
		glVertex2f(world[14], world[15]);
		glVertex2f(world[20], world[21]);
		glVertex2f(world[0], world[21]);
		glVertex2f(world[0], world[23]);
		glVertex2f(world[22], world[23]);
		glVertex2f(world[8], world[9]);
	glEnd();

	glBegin(GL_LINES);
		glVertex2f(world[16], world[17]);
		glVertex2f(world[18], world[19]);
	glEnd();

	float r = (world[10] - world[8])*0.164/2;
	glBegin(GL_LINE_LOOP);
		int i;
		for (i=0; i<360; i++) {
			glVertex2f(cos(i*DEG2RAD)*r + world[18], 
				   sin(i*DEG2RAD)*r + (world[17] + world[19])/2 );
		}
	glEnd();
}

/**
 * Cuida das funções drag and drop
 */
void mouseMotion(int x, int y) 
{
	//transforma coordenadas da tela em coordenadas do opengl
	float tx = 1000.0*x/640.0;
	float ty = 1000.0*(640.0 - y)/640.0; 
	
	if (a == 1) {
		float ballpos[2] = {tx + dx, ty + dy};
		phys->set_ballpos(ballpos);
	}
	else if (a != 0) {
		float robotpos[2] = {tx + dx, ty + dy};
		phys->set_robotpos(robotpos, a-2);
	}
}

/**
 * Função para capturar a posição do mouse
 */
void processMouse(int button, int state, int x, int y) {
	//transforma coordenadas da tela em coordenadas do opengl
	float tx = 1000.0*x/640.0;
	float ty = 1000.0*(640.0 - y)/640.0;
	
	if ( (state == GLUT_DOWN) && (button == GLUT_LEFT_BUTTON) ) {
		//se está em cima da bola
		float ballpos[2];
		phys->get_ballpos(ballpos);
		if (sqrt( pow(ballpos[0] - tx, 2) + pow(ballpos[1] - ty, 2) ) <= BALL_RADIUS + 10.0) {
			a = 1;

			dx = ballpos[0] - tx;
			dy = ballpos[1] - ty;
		}
		//se está em cima do robô
		else {
			int i;
			float robotpos[2];
			for (i = 0; i < NUM_ROB; ++i) {
				phys->get_robotpos(robotpos, i);
				if (sqrt( pow(robotpos[0] - tx, 2) + pow(robotpos[1] - ty, 2) ) <= ROBOT_RADIUS + 5.0) {
					a = i + 2;

					dx = robotpos[0] - tx;
					dy = robotpos[1] - ty;
					break;
				}
			}
		}
	}

	if ( (state == GLUT_UP) && (button == GLUT_LEFT_BUTTON) ) {
		a = 0;
	}

	if ( (state == GLUT_DOWN) && (button == GLUT_RIGHT_BUTTON) ) {
		float ballpos[2];
		phys->get_ballpos(ballpos);
			
		float ballvel[2] = {(tx - ballpos[0])*0.5, (ty - ballpos[1])*0.5};
		phys->set_ballvel(ballvel);
	}
}





/** Atualiza um vetor com dos dados de posições da bola e jogadores.
 * 
 * @param id_robot Número de identificação do robô de referência.
 *
 * @param *vector Ponteiro para o vetor que será atualizado com os dados do 
 * jogo (posição da bola e dos robôs e angulo dos robôs). <b>Este vetor deve 
 * estar alocado com pelo menos 32 posições antes da função ser chamada.</b>
 * Disposição dos dados:
 * 	|BALL_X|BALL_Y|POS1_X|POS1_Y|POS1_A|POS2_X|...|
 */
void 
GMSim::get_field( float * vector )
{
	phys->get_ballpos(vector);
	vector[0] *= CM2M;
	vector[1] *= CM2M;

	int i = 2, n = 0;
	float robotpos[2], robotang;
	while (i < NUM_ROB*3) {
		phys->get_robotpos(robotpos, n);
		phys->get_robotang(&robotang, n);

		vector[i++] = robotpos[0] * CM2M;
		vector[i++] = robotpos[1] * CM2M;
		vector[i++] = robotang;
		n++;
	}

}


/** Atualiza um vetor com dados sobre as dimensões do mundo.
 *
 * @param *vector Ponteiro para o vetor que será atualizado com os dados do 
 * mundo. <b>Este vetor deve estar alocado com pelo menos 28 posições antes 
 * da função ser chamada.</b>
 */
void 
GMSim::get_world( float * vector )
{
	phys->get_world(vector);

	int i;
	for (i=0; i<28; i++) {
		vector[i] *= CM2M;
	}
}

/** Seta a velocidade de um robô.
 *
 * @param id_robot O número do robô (de 0 a 4 robôs do nosso time e de 5 a
 * 9 são os robôs do time adversário).
 *
 * @param vx Velocidade no eixo X em m/s.
 *
 * @param vy Velocidade no eixo Y em m/s.
 *
 * @param va Velocidade angular em rad/s.
 */
void 
GMSim::robot_set_vel( int id_robot, 
		      float vx, 
		      float vy, 
		      float va )
{
	float robotvel[2] = {vx, vy};
	phys->set_robotvel(robotvel, va, id_robot);
}

/** Faz um robô chutar.
 *
 * @param id_robot O número do robô (de 0 a 4 robôs do nosso time e de 5 a
 * 9 são os robôs do time adversário).
 *
 * @param level Nível (força) do chute.
 */
void 
GMSim::robot_kick( int id_robot, 
		   int level )
{
	phys->robot_kick( level, id_robot );
}

/** Ativa ou desativa o rolinho de um determinado robô.
 *
 * @param id_robot O número do robô (de 0 a 4 robôs do nosso time e de 5 a
 * 9 são os robôs do time adversário).
 *
 * @param enable Se for 0 desativa o rolinho, caso contrário ativa o rolinho.
 */
void 
GMSim::robot_dribble_enable( int id_robot, 
			     int enable )
{
	phys->robot_dribble( enable, id_robot );
}

/**  
 * Destrói o simulador.
 */
void 
GMSim::destroy( void )
{
	pthread_cancel(p);
	pthread_join(p, 0);
}

